// (C) Szymon Piechowicz, February 2012

#include "obstacle_detector.h"
#include <avr/interrupt.h>
#include "common.h"


#define OBSTACLE_DETECTOR_IR_LED_PORT PORTD
#define OBSTACLE_DETECTOR_IR_LED_PORT_DIR DDRD
#define OBSTACLE_DETECTOR_IR_LED_BIT PD7 // OC2
#define OBSTACLE_DETECTOR_IR_RECEIVER_PORT PORTD
#define OBSTACLE_DETECTOR_IR_RECEIVER_PORT_DIR DDRD
#define OBSTACLE_DETECTOR_IR_RECEIVER_PORT_IN PIND
#define OBSTACLE_DETECTOR_IR_RECEIVER_BIT PD2 // INT0


static obstacle_detector_obstacle_callback obstacle_callback;
// Callbacks are fired only when obstacle state changes.
static bool obstacle_did_report_first_result;
static bool obstacle_last_reported_result;


void obstacle_detector_init(void)
{
  PORT_OFF(OBSTACLE_DETECTOR_IR_LED_PORT, OBSTACLE_DETECTOR_IR_LED_BIT);
  PORT_DIR_OUT(OBSTACLE_DETECTOR_IR_LED_PORT_DIR, OBSTACLE_DETECTOR_IR_LED_BIT);
  PORT_ON(OBSTACLE_DETECTOR_IR_RECEIVER_PORT,
      OBSTACLE_DETECTOR_IR_RECEIVER_BIT); // pull-up
  
  // OC2 toggling frequency: 16 MHz / (2 * 1 * (1 + 241)) = 33 058 Hz
  TCCR2 |= (1 << WGM21); // clear timer on compare match
  OCR2 = 241; // clear timer on 241
  MCUCR |= (1 << ISC00); // logical change on INT0 causes interrupt
}

void obstacle_detector_start(obstacle_detector_obstacle_callback callback)
{
  obstacle_callback = callback;
  obstacle_did_report_first_result = false;
  TCCR2 |= (1 << COM20); // toggle OC2 on timer compare match
  TCCR2 |= (1 << CS20); // prescaler / 1
  GICR |= (1 << INT0); // enable interrupt for INT0
}

void obstacle_detector_stop(void)
{
  TCCR2 &= ~(1 << COM20); // disconnect OC2
  TCCR2 &= ~(1 << CS20); // no clock source, timer stopped
  GICR &= ~(1 << INT0); // disable interrupt for INT0
}

ISR(INT0_vect)
{
  bool is_obstacle_detected = !PORT_GET(OBSTACLE_DETECTOR_IR_RECEIVER_PORT_IN,
      OBSTACLE_DETECTOR_IR_RECEIVER_BIT);
  if (!obstacle_did_report_first_result
      || is_obstacle_detected != obstacle_last_reported_result)
  {
    obstacle_did_report_first_result = true;
    obstacle_last_reported_result = is_obstacle_detected;
    obstacle_callback(is_obstacle_detected);
  }
}